#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/cloud_viewer.h>

// 全局变量：PCL可视化工具
pcl::visualization::CloudViewer viewer("PointCloud Viewer");

// 回调函数：接收并显示点云数据
void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) {
    // 将ROS点云消息转换为PCL点云格式
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::fromROSMsg(*msg, *cloud);

    // 显示点云
    if (!viewer.wasStopped()) {
        viewer.showCloud(cloud);
    }
}

int main(int argc, char** argv) {
    // 初始化ROS节点
    ros::init(argc, argv, "pointcloud_display_node");

    // 创建节点句柄
    ros::NodeHandle nh;

    // 订阅点云话题
    ros::Subscriber pointcloud_sub = nh.subscribe("/rslidar_points", 10, pointCloudCallback);

    // ROS循环
    ros::spin();

    return 0;
}